%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sajad Saeedi
% University of New Brunswick
% Copy Right 2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ref: 
% S. Saeedi, L. Paull, M. Trentini, M. Seto, and H. Li, 
% Map Merging Using Hough Peak Matching, 
% Proceedings of the IEEE/RSJ International Conference on 
% Intelligent Robots and Systems (IROS), 
% Vilamoura, Portugal, October 7-12, pp. 4683 - 4688.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function plots two maps on the same canvus 
%
% INPUT: 
% - map1: input map1 
% - map2: input map2 
% - clims: scaling for colors
%
% OUTPUT:
% - none
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function m1 = plotbothmaps(m1, m2, clims)
    figure;
    hold on;
    %set(gco, 'Ydir', 'normal');
%     n = 400;
%     xlim([0-n/100 n+n/100]);ylim([0-n/100 n+n/100]); 
     
     %n1 = 640;
     %n2 = 480;
     %xlim([0-n1/100 n1+n1/100]);ylim([0-n2/100 n2+n2/100]);
     
    [nr nc] = size(m2);
    for r = 1:nr
       for c = 1:nc
%             if m2(r,c) ~= 0 %  %  for other than ros-gazebo
%                 m1(r,c) = m2(r,c)+m1(r,c); 
%             end

             if m2(r,c) < 0 && m1(r,c) < 0 % added for gazebo
                m1(r,c) = m2(r,c) + m1(r,c);
             end
             if m2(r,c) > 0 && m1(r,c) > 0 % added for gazebo
                m1(r,c) = max(m2(r,c) , m1(r,c));
             end
             if m2(r,c) ~= 0 && m1(r,c) == 0 % added for gazebo
                m1(r,c) = m2(r,c);
             end
       end
    end

    imshow(m1, [-1 1])
    % xlim([0 n]);ylim([0 n]);
    % clims = [-1 1];
    %imagesc(m1, clims);
    %colormap(gray)
    %axis square
    %axis([1 170 1 170]);
    hold off
